﻿#include "opencv2/opencv.hpp"
#include <iostream>
#include <fstream>
#include <vector>
#include <thread>
#include <Classifier/torch.h>
#include <ArmorBoxDetector/findArmorBoxes.h>
#include <manifoldCommunication.h>
#include <PowerRuneDetector.h>
#include <KalmanPredictor.h>
#include <Camera.h>
#include <Log/output.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <mutex>
#include <sys/time.h>
#include <linux/rtc.h>
#include <time.h>
#include "socket.hpp"
#include "output.h"
#include "customTimer.h"

using namespace cv;
using namespace std;
using namespace robowalker;


//#define DEBUG

// #define PROFILE_READ_IMG_TIME
#define RECORD_VIDEO

int main()
{
    // 初始化摄像机
    const double expectedFrameRate = 200;
    const int exposureTime = 3500;
    const int imgHeight = 1024;
    const int imgWidth = 1280;
    auto cam = Camera("Camera/data/out_camera_data.xml", expectedFrameRate, exposureTime, imgHeight, imgWidth);
    printf("cam.is_open() = %d\n", cam.is_open());
    if(cam.is_open() == false)
    {   
        printf("cam.is_open() == false\n");
        return 1; 
    }
        
    // 初始化装甲板检测器  
    string modelfile = "ArmorBox/data/Classifier/model.json";
    string paramsfile = "ArmorBox/data/ArmorBoxDetector/params.json";
    ArmorBoxesDetector detector(imgWidth, imgHeight, modelfile, 4,  paramsfile);
    
    vector<ArmorBox> rst;
    vector<RotatedRect> blobs;
    vector<ArmorBox> suspects;

    // 初始化能量机关检测器
    auto fps = cam.getCurrentFrameRate();
    PowerRuneDetector prDetector(fps);
    
#ifdef RECORD_INFO    
    outfile.open("./records/"+asyncRecorder::timestr()+".txt", ios::app | ios::in | ios::out);
#endif

#ifdef RECORD_VIDEO
    asyncRecorder recorder("/media/dji/82a14793-5d11-41af-b4f0-4b181aa7fba81/records/" + asyncRecorder::timestr()+"/");
#endif

    // 开始读取相机内容
    int imgNum = 0;
    Mat bayerRG8Img; 

    car_info car_data;

    // bool is_kalman_reset = false;
    Mat angleMeasurement = Mat(2, 1, CV_32F, Scalar::all(0));
    Mat predictAngle = Mat(2, 1, CV_32F, Scalar::all(0));
    

    double distance; 
    ArmorBox selectedArmorBox;
    int detectState = 0;  

    // Start a new thread for kalman filter predicting & message receive and send 
    //std::thread kalmanPredictThread(thread_kalmanPredictor, &angleMeasurement, &predictAngle, &is_kalman_reset, &distance, &car_data, &detectState); // TODO
    //kalmanPredictThread.detach();
    
    double last_t = getTickCount();
    double t, dt, t1, t2;

    const char *manifoldSerialPath = "/dev/ttyTHS2";
    const int baudrate = 115200;
    CommunicationManager CM(manifoldSerialPath, baudrate);

    const size_t encoder_angle_seq_size = 10;
    float encoder_angle [encoder_angle_seq_size][2];
    float target_angle[2];
    memset(encoder_angle,0,sizeof(encoder_angle_seq_size));

    while(1)
    {
        // t = getTickCount();
        // dt = (double)(t - last_t) / getTickFrequency() * 1000;
        // printf("one loop time: %lf\n", dt);
        // last_t = t;

        // t1 = getTickCount();
        int64_t imgtimestamp;
        auto img = cam.readOneFrame(bayerRG8Img, imgtimestamp);
        double imgtime = gettime();
        //printf("img time %9.9lf \n", gettime());
        //outfile<<"img time\t"<<gettime()<<endl;

        // t2 = getTickCount();
        // dt = (t2 - t1) / getTickFrequency() * 1000;
        // print("readOneFrame time: %lf\n", dt);
        

        Mat srcImg;
        if(nullptr == img)
        {
            printf("[Error] Read no image from camera.\n");
            break;
        } else if(bayerRG8Img.empty())
        {
            printf("[Error] Image read from camera is empty!\n");
            break;
        }
        // 读取成功，
        imgNum ++;
        cvtColor(bayerRG8Img, srcImg, COLOR_BayerRG2RGB);

        string str_color = car_data.color ? "blue" : "red"; 
        bool is_shooting_PowerRune = car_data.is_shooting_energy;

        // string powerRune_str_color = car_data.color ? "red" : "blue"; // Color of power rune is the same as our own color 
        // detector.setEnemy(str_color); 
        // prDetector.setPowerRuneColor(powerRune_str_color);
        prDetector.setPowerRuneColor("red"); //TODO: for competition


        if (CM.manifoldReceiveOnce(&car_data) > 0)
        {
            //printf("angle time %9.9lf \n", gettime());
            for(int i = encoder_angle_seq_size -1; i>0; --i)
            {
                encoder_angle[i][0] = encoder_angle[i-1][0];
                encoder_angle[i][1] = encoder_angle[i-1][1];
            }
            const float delta_pitch = -268.8;
            const float delta_yaw = 5.0;
            encoder_angle[0][0] = car_data.pitch + delta_pitch;
            encoder_angle[0][1] = car_data.yaw + delta_yaw;
#ifdef RECORD_INFO
            char str[100];
            sprintf(str, "decoder info:\t%9.9lf\t%f\t%f\n",gettime(),car_data.pitch, car_data.yaw);
            outfile<<str;
#endif
        }
        else
        {
            printf("[ CM.manifoldReceiveOnce(&car_data) ] Not received! \n");
            // usleep(10);
        }

        // t1 = getTickCount();
        
        //printf("color: %s\n", str_color.c_str()); 

        if(false == is_shooting_PowerRune) // Detecting ArmorBox mode 
        {
            detector(srcImg, rst, blobs, suspects);
            // t2 = getTickCount();
            // dt = (t2 - t1) / getTickFrequency() * 1000;
            // print("detect time: %lf\n", dt);
            // Mat showImg;
            // t1 = getTickCount();
            // detector.draw(srcImg, showImg, rst, blobs, suspects);       
            // t2 = getTickCount();     
            // dt = (t2 - t1) / getTickFrequency() * 1000;
            // print("draw time: %lf\n", dt);
            // display("Camera Capture", srcImg);

#ifdef DEBUG
            
            // imshow("test", srcImg);
            // waitKey(1) ;
    //            LOG("dt" + to_string(dt) + "ms\n");
    //            output();
            t1 = getTickCount();
            detector.draw(srcImg, showImg, rst, blobs, suspects);            
            imshow("Camera Capture", showImg);
            cv::waitKey(1);
            t2 = getTickCount();
            dt = (t2 - t1) / getTickFrequency() * 1000;
            printf("imshow time: %lf\n", dt);
#endif
            selectedArmorBox = detector.SelectArmorBoxes(rst, selectedArmorBox, cam);
            detectState = detector.getDetectState();

            if(!selectedArmorBox.isNull())
            {
                distance = cam.getDistance(ARMORBOX_SMALL, selectedArmorBox.vertices);
                auto angleRst = cam.getAngle(selectedArmorBox.getCenter());
                angleMeasurement.at<float>(0) = angleRst.first;
                angleMeasurement.at<float>(1) = angleRst.second;
            } else{
                angleMeasurement.at<float>(0) = 0.0;
                angleMeasurement.at<float>(1) = 0.0;
            }
        }

        // else if(true == is_shooting_PowerRune)
        //  namedWindow("1. origin", WINDOW_NORMAL);
            // imshow("1. origin", srcImg);
        if(true == is_shooting_PowerRune)
        {
            printf("true == is_shooting_PowerRune\n");

#ifdef RECORD_VIDEO
            bool rst = recorder.record(srcImg); // 改成只在击打能量机关的时候录制视频
            if(!rst)
                printf("recorder.record(srcImg): false\n"); 
#endif
           

            // printf("[Status] is shooting PowerRune\n");
            double distance = 8000; // 8000mm = 8.0m 
            Point2f targetPoint{0.0, 0.0}; // predict 
            Point2f detectPoint{0.0, 0.0}; // measurement 
            auto t1 = getTickCount();

            const int delta_T = 2;

            Point2f delta_p = cam.getPixelSize(-encoder_angle[delta_T][1], -encoder_angle[delta_T][0]);
            //printf("delta_p [%f, %f]\n", delta_p.x, delta_p.y);
           
            const double delay_s = 0.2; 
            bool detect_success = prDetector.detect(srcImg, 0, delay_s, delta_p.x, delta_p.y, detectPoint, targetPoint);
            //std::cout<<targetPoint<<"  "<<detect_success<<endl;
            auto t2 = getTickCount();
            dt = (t2 - t1) / getTickFrequency() * 1000;
            //printf("detect time: %lf\n", dt);
            //printf("%f,%f,%f,%f\n",angleMeasurement.at<float>(0),angleMeasurement.at<float>(1),car_data.pitch, car_data.yaw);

            static int failure_cnt = 0;

            if(detect_success && !isnan(targetPoint.x))
            {    
                auto angleRst = cam.getAngle(detectPoint);
                angleMeasurement.at<float>(0) = angleRst.first;
                angleMeasurement.at<float>(1) = angleRst.second;

                auto predictAngleRst = cam.getAngle(targetPoint);
                predictAngle.at<float>(0) = predictAngleRst.first;
                predictAngle.at<float>(1) = predictAngleRst.second; 
                failure_cnt = 0;

                // target_angle[0] = angleMeasurement.at<float>(1) - encoder_angle[2][0];
                // target_angle[1] = angleMeasurement.at<float>(0) - encoder_angle[2][1];
            }
            else
            {
                ++failure_cnt;
                if (failure_cnt  > 20)
                {
                    angleMeasurement.at<float>(0) = angleMeasurement.at<float>(1) = 0;
                    predictAngle.at<float>(0) = predictAngle.at<float>(1) = 0.;
                    target_angle[0] =  target_angle[1] = 0;
                }
            }

#ifdef RECORD_INFO
            char str[100];
            sprintf(str, "vision info:\t%9.9lf\t%f\t%f\t%ld\n",imgtime,angleMeasurement.at<float>(0),angleMeasurement.at<float>(1),imgtimestamp);
            outfile<<str;
            //outfile<<"vision info:\t"<<gettime()<<"\t"<<angleMeasurement.at<float>(0)<<"\t"<<angleMeasurement.at<float>(1)<<endl;
#endif
            // print("delta angle [%f\t%f]\n",angleMeasurement.at<float>(0), angleMeasurement.at<float>(1));
            CM.manifoldSendOnce(assembleVisionInfoStruct(angleMeasurement.at<float>(0), angleMeasurement.at<float>(1), 
                                                        predictAngle.at<float>(0), predictAngle.at<float>(1), 
                                                        0, 0, 0));
            if(detect_success && fabs(angleMeasurement.at<float>(0)) < 1e-5) // debug
            {
                // printf("target point: [%f %f]\n", targetPoint.x, targetPoint.y);
                print("delta angle [%f\t%f]\n",angleMeasurement.at<float>(0), angleMeasurement.at<float>(1));
            }
        }
    }

    return 0;
}


